﻿//
// Created by QiChen on 2021/4/12.
//

#include "ekf_a.h"
//#define LOG_OUT
void EKF_A::update(Vector3d acc) {

//    double  F = 1;
    double P1 = P + Q;
//    double
//    rotation_matrix = AngleAxisd(angle,(Vector3d()<<1,0,0).finished()).toRotationMatrix().transpose();
    rotation_matrix = AngleAxisd(angle,r_axis).toRotationMatrix().transpose();
    acc_m = rotation_matrix*acc_init;
#ifdef LOG_OUT
    cout << "rotation_matrix"<<endl;
    cout << rotation_matrix<<endl;
    cout << "acc_m"<<endl;
    cout << acc_m<<endl;
#endif
    double xx = r_axis[0]*r_axis[0],x = r_axis[0];
    double yy = r_axis[1]*r_axis[1],y = r_axis[1];
    double zz = r_axis[2]*r_axis[2],z = r_axis[2];
    double xy = r_axis[0]*r_axis[1];
    double xz = r_axis[0]*r_axis[2];
    double yz = r_axis[1]*r_axis[2];
    double ss = sin(angle), cc = cos(angle);
    Hx << (Matrix3d()<<-ss+xx*ss, xy*ss + z*cc, xz*ss-y*cc,
                    xy*ss-z*cc, -ss+yy*ss, yz*ss + cc*x,
                    xz*ss+y*cc,y/z*ss-x*cc, -ss+zz*ss).finished()*acc_init;
    RowVector3d Hx_t = Hx.transpose();
#ifdef LOG_OUT
    cout << "Hx"<<endl;
    cout << Hx<<endl;
    cout << "Hx_t"<<endl;
    cout << Hx_t<<endl;
#endif
    K = P1*Hx_t*((Hx*P1*Hx_t+ R).inverse());
    angle =angle+ K*(acc - acc_m);
    P = (1 - K*Hx) * P1;
#ifdef LOG_OUT
    cout << "K"<<endl;
    cout << K<<endl;
    cout << "P"<<endl;
    cout << P<<endl;
    cout << "---"<<endl;
    cout << acc - acc_m<<endl;
    cout << angle<<endl;
#endif
}
